#include <AccelStepper.h>

// ================== PIN DEFINITIONS ==================
#define STEP_PIN D1
#define DIR_PIN  D0
#define EN_PIN   D3
#define M0_PIN   D2
#define M1_PIN   D4
#define M2_PIN   D5
#define HALL_PIN D8

// ================== HALL THRESHOLDS ==================
#define ON_THRESHOLD  3800
#define OFF_THRESHOLD 3400

// ================== TURNTABLE CONSTANTS ==================
#define MOTOR_STEPS     200
#define MICROSTEPS      16
#define GEAR_RATIO      5
#define STEPS_PER_REV   (MOTOR_STEPS * MICROSTEPS * GEAR_RATIO)  // 16,000

// ================== SPEED SETTINGS ==================
#define SEARCH_SPEED    800
#define BACKOFF_SPEED   400
#define APPROACH_SPEED  200
#define MOVE_MAX_SPEED  1200
#define MOVE_ACCEL      600

// ================== CALIBRATED LOOKUP TABLE ==================
// Compartments physically numbered CCW, turntable rotates CW
// CW travel order: C1 → C6 → C5 → C4 → C3 → C2 → C1
//
//                     [0]   [H1]    [H2]    [H3]    [H4]
const long LUT[7][5] = {
  {0,      0,      0,      0,      0     }, // unused
  {0,      0,   4000,   8000,  12000    }, // C1 (  0° CW from C1)
  {0,   2665,   6665,  10665,  14665    }, // C2 (300° CW from C1)
  {0,   5332,   9332,  13332,   1332    }, // C3 (240° CW from C1)
  {0,   7999,  11999,  15999,   3999    }, // C4 (180° CW from C1)
  {0,  10666,  14666,   2666,   6666    }, // C5 (120° CW from C1)
  {0,  13333,   1333,   5333,   9333    }, // C6 ( 60° CW from C1)
};

const char* COMP_LABEL[7] = {"", "C1","C2","C3","C4","C5","C6"};
const char* HOME_LABEL[5] = {"", "H1","H2","H3","H4"};

// ================== STEPPER ==================
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

// ================== HALL STATE ==================
float filtered  = 0.0f;
bool  hallState = false;

// ================== STATE MACHINES ==================
enum HomingState { SEARCH, BACKOFF, APPROACH, HOMING_DONE };
enum SystemState { HOMING, POSITIONING, IDLE };

HomingState homingState = SEARCH;
SystemState sysState    = HOMING;

// ================== POSITION TRACKER ==================
long turntableStepPos = 0;
int  lastComp         = 0;
int  lastHome         = 0;

// ================== HALL SENSOR ==================
bool readHallDigital() {
  int raw = analogRead(HALL_PIN);
  filtered = 0.8f * filtered + 0.2f * (float)raw;
  if (!hallState && filtered > ON_THRESHOLD)  hallState = true;
  if ( hallState && filtered < OFF_THRESHOLD) hallState = false;
  return hallState;
}

// ================== HOMING ==================
void runHoming() {
  static bool lastHall = false;
  bool currentHall = readHallDigital();

  switch (homingState) {

    case SEARCH:
      stepper.setSpeed(SEARCH_SPEED);
      stepper.runSpeed();
      if (currentHall && !lastHall) {
        Serial.println("[HOMING] Magnet found → backing off...");
        homingState = BACKOFF;
      }
      break;

    case BACKOFF:
      stepper.setSpeed(-BACKOFF_SPEED);
      stepper.runSpeed();
      if (!currentHall && lastHall) {
        Serial.println("[HOMING] Exited zone → slow approach...");
        homingState = APPROACH;
      }
      break;

    case APPROACH:
      stepper.setSpeed(APPROACH_SPEED);
      stepper.runSpeed();
      if (currentHall && !lastHall) {
        stepper.setSpeed(0);
        stepper.setCurrentPosition(0);
        turntableStepPos = 0;
        lastComp         = 1;
        lastHome         = 1;

        homingState = HOMING_DONE;
        sysState    = IDLE;

        stepper.setMaxSpeed(MOVE_MAX_SPEED);
        stepper.setAcceleration(MOVE_ACCEL);

        Serial.println("╔══════════════════════════════╗");
        Serial.println("║   HOMING COMPLETE — C1 = H1  ║");
        Serial.println("╚══════════════════════════════╝");
        printHelp();
      }
      break;

    case HOMING_DONE:
      break;
  }

  lastHall = currentHall;
}

// ================== MOVE — ALWAYS CW ==================
void moveCompartmentToHome(int c, int h) {
  if (c < 1 || c > 6) { Serial.println("[ERR] Compartment must be 1-6"); return; }
  if (h < 1 || h > 4) { Serial.println("[ERR] Home must be 1-4");        return; }

  long needed = LUT[c][h];

  // Always CW — if target is behind current pos, wrap around
  long delta = needed - turntableStepPos;
  if (delta < 0)  delta += STEPS_PER_REV;
  if (delta == 0) {
    Serial.println("[INFO] Already at target.");
    sysState = IDLE;
    return;
  }

  turntableStepPos = needed;
  lastComp         = c;
  lastHome         = h;

  stepper.moveTo(stepper.currentPosition() + delta);
  sysState = POSITIONING;

  Serial.println("┌──────────────────────────────┐");
  Serial.print  ("│  Moving "); Serial.print(COMP_LABEL[c]);
  Serial.print  (" → ");        Serial.println(HOME_LABEL[h]);
  Serial.println("├──────────────────────────────┤");
  Serial.print  ("│  LUT target   : "); Serial.println(needed);
  Serial.print  ("│  Steps CW     : "); Serial.println(delta);
  Serial.print  ("│  Degrees CW   : ");
  Serial.print  (delta * 360.0f / STEPS_PER_REV, 2);
  Serial.println("°");
  Serial.println("│  Direction    : CW →          │");
  Serial.println("└──────────────────────────────┘");
}

// ================== PRINT LUT ==================
void printLUT() {
  Serial.println("\n╔════════════════════════════════════╗");
  Serial.println("║      CALIBRATED LOOKUP TABLE       ║");
  Serial.println("║  CW order: C1→C6→C5→C4→C3→C2→C1  ║");
  Serial.println("╠════════╦═══════╦═══════╦═══════╦═══════╣");
  Serial.println("║        ║  H1   ║  H2   ║  H3   ║  H4   ║");
  Serial.println("╠════════╬═══════╬═══════╬═══════╬═══════╣");
  for (int c = 1; c <= 6; c++) {
    Serial.print("║  ");
    Serial.print(COMP_LABEL[c]);
    Serial.print("     ║");
    for (int h = 1; h <= 4; h++) {
      char buf[8];
      sprintf(buf, "%6ld ", LUT[c][h]);
      Serial.print(buf);
      Serial.print("║");
    }
    Serial.println();
  }
  Serial.println("╚════════╩═══════╩═══════╩═══════╩═══════╝\n");
}

// ================== PRINT HELP ==================
void printHelp() {
  Serial.println("\n╔══════════════════════════════╗");
  Serial.println("║         COMMANDS             ║");
  Serial.println("╠══════════════════════════════╣");
  Serial.println("║  C<n>H<m>  move compartment  ║");
  Serial.println("║  e.g. C1H1  C3H2  C6H4       ║");
  Serial.println("╠══════════════════════════════╣");
  Serial.println("║  HOME   re-run homing        ║");
  Serial.println("║  POS    show position        ║");
  Serial.println("║  LUT    show lookup table    ║");
  Serial.println("║  HELP   show this menu       ║");
  Serial.println("╚══════════════════════════════╝\n");
}

// ================== PARSE SERIAL ==================
void parseCommand(String cmd) {
  cmd.trim();
  cmd.toUpperCase();

  // C<n>H<m> format
  if (cmd.length() == 4 && cmd[0] == 'C' && cmd[2] == 'H') {
    int c = cmd[1] - '0';
    int h = cmd[3] - '0';
    moveCompartmentToHome(c, h);
    return;
  }

  if (cmd == "HOME") {
    homingState      = SEARCH;
    sysState         = HOMING;
    turntableStepPos = 0;
    lastComp         = 0;
    lastHome         = 0;
    stepper.setMaxSpeed(SEARCH_SPEED);
    Serial.println("[HOMING] Starting...");
    return;
  }

  if (cmd == "POS") {
    float deg = turntableStepPos * (360.0f / STEPS_PER_REV);
    Serial.println("\n╔══════════════════════════════╗");
    Serial.println("║         POSITION             ║");
    Serial.println("╠══════════════════════════════╣");
    Serial.print  ("║  Steps  : "); Serial.println(turntableStepPos);
    Serial.print  ("║  Angle  : "); Serial.print(deg, 2); Serial.println("°");
    if (lastComp > 0) {
      Serial.print("║  Last   : ");
      Serial.print(COMP_LABEL[lastComp]);
      Serial.print(" → ");
      Serial.println(HOME_LABEL[lastHome]);
    }
    Serial.println("╚══════════════════════════════╝\n");
    return;
  }

  if (cmd == "LUT")  { printLUT();  return; }
  if (cmd == "HELP") { printHelp(); return; }

  Serial.print("[ERR] Unknown: "); Serial.println(cmd);
  Serial.println("      Type HELP for commands");
}

// ================== SETUP ==================
void setup() {
  Serial.begin(115200);

  pinMode(EN_PIN, OUTPUT);  digitalWrite(EN_PIN, LOW);

  // 1/16 microstepping — DRV8825
  // M0=LOW, M1=LOW, M2=HIGH
  pinMode(M0_PIN, OUTPUT);  digitalWrite(M0_PIN, LOW);
  pinMode(M1_PIN, OUTPUT);  digitalWrite(M1_PIN, LOW);
  pinMode(M2_PIN, OUTPUT);  digitalWrite(M2_PIN, HIGH);

  filtered = analogRead(HALL_PIN);
  stepper.setMaxSpeed(SEARCH_SPEED);

  Serial.println("\n╔══════════════════════════════╗");
  Serial.println("║   TURNTABLE CONTROLLER v6    ║");
  Serial.println("║   Always CW — CCW numbered   ║");
  Serial.println("║   NEMA17 + DRV8825 + 5:1     ║");
  Serial.println("╚══════════════════════════════╝");
  Serial.println("Starting homing sequence...\n");
}

// ================== LOOP ==================
void loop() {
  switch (sysState) {

    case HOMING:
      runHoming();
      break;

    case POSITIONING:
      if (stepper.distanceToGo() != 0) {
        stepper.run();
      } else {
        Serial.print("\n[DONE] ");
        Serial.print(COMP_LABEL[lastComp]);
        Serial.print(" is now at ");
        Serial.println(HOME_LABEL[lastHome]);
        Serial.println("       Ready for next command.\n");
        sysState = IDLE;
      }
      break;

    case IDLE:
      if (Serial.available()) {
        String cmd = Serial.readStringUntil('\n');
        parseCommand(cmd);
      }
      break;
  }
}